/*
 * cmd.cpp
 *
 *  Created on: 08.02.2012
 *      Author: Roma Jam
 */
#include "main.h"
#include "delay_util.h"
#include "cmd.h"
#include "usart.h"
#include "lcd_init.h"
#include "motors.h"
#include "adc.h"
#include "events.h"
#include "set_angles.h"

cmdvariables_t CMD_Var;

void cmd_t::Reset() {
    CMD_Var.counter = 0;
    CMD_Var.string = 0;
    CMD_Var.cmd_byte = 0;
    CMD_Var.pointer = 0;
} // Reset

void cmd_t::Terminal_Process_Byte(uint8_t Byte) {
    if (Byte == 'B') {
        USART.Print_String("Switch to LabServer - OK\r");
        parametres.cmd_sourse = LabServer;
        parametres.cmd_switch_pointer = false;
        Active_window.Search(window.pointer);
        return;
    }
    if (Byte == ' ') {
        Reset();
        USART.Print_String("Type a new command please:\r");
    }
    else {
        CMD_Var.Arr[CMD_Var.counter] = Byte;
        CMD_Var.counter++;
            if (CMD_Var.counter > lengh_of_cmd-1) {
//                    && (Byte == '\r')) {
                Reset();
                Terminal_Process_Cmd();
                Terminal_Process_Cmd_Event(CMD_Var.pointer);
                Clear_buffer();
            }
    }
} // Terminal Process Byte
void cmd_t::Terminal_Angle_Setup(uint8_t Byte) {
    extern uint16_t new_azimuth, new_elevation;
    if (Byte == 'Q') {
        return;
    } else {
        if (Byte == 'Y') {
            uint16_t az01, az02, az03, el01, el02;
            az01 = CMD_Var.azimuth_Arr[0];
            az01 = ADC.Char_to_Uint(az01);
            az02 = CMD_Var.azimuth_Arr[1];
            az02 = ADC.Char_to_Uint(az02);
            az03 = CMD_Var.azimuth_Arr[2];
            az03 = ADC.Char_to_Uint(az03);
            el01 = CMD_Var.elevation_Arr[0];
            el01 = ADC.Char_to_Uint(el01);
            el02 = CMD_Var.elevation_Arr[1];
            el02 = ADC.Char_to_Uint(el02);
            new_azimuth = ((az01*100)+(az02*10)+az03);
            new_elevation = ((el01*10)+el02);
            USART.Print_Uint(new_azimuth);
            USART.Print('\r');
            USART.Print_Uint(new_elevation);
            USART.Print('\r');
            return;
        }
        if (Byte == 'N') {
            USART.Print_String("Angles reset.\r");
            new_azimuth = 0;
            new_elevation = 0;
            angle_flag.az_full = false;
            angle_flag.el_full = false;
            CMD_Var.az_counter = 0;
            CMD_Var.el_counter = 0;
        }
        if (Byte == ' '){
            if ((!angle_flag.az_full) && (!angle_flag.el_full)) {
                USART.Print_String("Insert new value of azimuth angle (0..60 or 300..360):\r");
            }
            if ((angle_flag.az_full) && (!angle_flag.el_full)) {
                USART.Print_String("Insert new value of elevation angle (0..30):\r");
            }
            if ((angle_flag.az_full) && (angle_flag.el_full)) {
                USART.Print_String("Angles are setup:\r");
                uint8_t i;
                USART.Print_String("Azimuth: ");
                for (i=0; i<az_lengh; i++) {
                    USART.Print(CMD_Var.azimuth_Arr[i]);
                }
                USART.Print('\r');
                USART.Print_String("Elevation: ");
                for (i=0; i<el_lengh; i++) {
                    USART.Print(CMD_Var.elevation_Arr[i]);
                }
                USART.Print_String("\rSetup angles Y/N?\r");
            }
        }
            else {
                if ((!angle_flag.az_full) && (!angle_flag.el_full)) {
                    CMD_Var.azimuth_Arr[CMD_Var.az_counter] = Byte;
                    CMD_Var.az_counter++;
                    if (CMD_Var.az_counter > az_lengh-1) {
                        angle_flag.az_full = true;
                        USART.Print_String("Azimuth is set.\r");
                        return;
                    }
                }
                if ((angle_flag.az_full) && (!angle_flag.el_full)) {
                    CMD_Var.elevation_Arr[CMD_Var.el_counter] = Byte;
                    CMD_Var.el_counter++;
                    if (CMD_Var.el_counter > el_lengh-1) {
                        angle_flag.el_full = true;
                        USART.Print_String("Elevation is set.\r");
                        return;
                    }
                }
            }
    }
} // Terminal Angle Setup
void cmd_t::Terminal_Process_Cmd(void) {
    uint8_t i,j;

    for (i=0; i<number_of_cmd; i++) {
        CMD_Var.string++;
        for (j=0; j<lengh_of_cmd; j++) {
            if (CMD_Var.Arr[j] != array_of_cmd[i][j]) {
                CMD_Var.cmd_byte = 0;
            } else {
                CMD_Var.cmd_byte++;
            }
        } // for j

            if (CMD_Var.cmd_byte == (lengh_of_cmd)) {
                CMD_Var.pointer = CMD_Var.string;
            } // if

    } // for i
} // Terminal Process Cmd

void cmd_t::Terminal_Process_Cmd_Event(uint8_t Apointer) {
    switch (Apointer) {
        case 0: { // Wrong cmd
            USART.Print_String("Insert wrong command, please use next:\r");
            uint8_t i,j;

            for (i=0; i<number_of_cmd; i++) {
                for (j=0; j<lengh_of_cmd; j++) {
                    USART.Print(array_of_cmd[i][j]);
                } // for j
                USART.Print('\r');
            } // for i
        }
        break;
        case 1: { // ShowAngle
            USART.Print_String("Angles measures:\r");
            USART.Print_String("Azimuth: ");
            USART.Print_Uint(adc01_angle);
            USART.Print_String(" ");
            USART.Print_Uint(adc01_minute);
            USART.Print('\r');
            USART.Print_String("Elevation: ");
            USART.Print_Uint(adc02_angle);
            USART.Print_String(" ");
            USART.Print_Uint(adc02_minute);
            USART.Print('\r');
        }
        break;
        case 2: { // SetupMode
            USART.Print_String("Entered to 'Setup Mode'. For quit press Q.\r");
            Set_Parametres();
        }
        break;
        case 3: { // Channel01
            USART.Print_String("Parametres of Channel 01 (Azimuth):\r");
            USART.Print_String("ADC measure: ");
            USART.Print_Uint(adc01_data);
            USART.Print('\r');
            USART.Print_String("Angle: ");
            USART.Print_Uint(adc01_angle);
            USART.Print('\r');
            USART.Print_String("Minute: ");
            USART.Print_Uint(adc01_minute);
            USART.Print('\r');
        }
        break;
        case 4: { // Channel02
            USART.Print_String("Parametres of Channel 02 (Elevation):\r");
            USART.Print_String("ADC measure: ");
            USART.Print_Uint(adc02_data);
            USART.Print('\r');
            USART.Print_String("Angle: ");
            USART.Print_Uint(adc02_angle);
            USART.Print('\r');
            USART.Print_String("Minute: ");
            USART.Print_Uint(adc02_minute);
            USART.Print('\r');
        }
        break;
        case 5: { // MotorUpOn
            MotorUp_On();
            USART.Print_String("Motor UP (Elevation down) is On now.\r");
        }
        break;
        case 6: { // MotorUpOf
            MotorUp_Off();
            USART.Print_String("Motor UP (Elevation down) is Off now.\r");
        }
        break;
        case 7: { //MotorDnOn
            MotorDown_On();
            USART.Print_String("Motor DOWN (Elevation up) is On now.\r");
        }
        break;
        case 8: { // MotorDnOf
            MotorDown_Off();
            USART.Print_String("Motor DOWN (Elevation up) is Off now.\r");
        }
        break;
        case 9: { // MotorLtOn
            MotorLeft_On();
            USART.Print_String("Motor LEFT (Azimuth left) is On now.\r");
        }
        break;
        case 10: { // MotorLtOf
            MotorLeft_Off();
            USART.Print_String("Motor LEFT (Azimuth left) is Off now.\r");
        }
        break;
        case 11: { // MotorRtOn
            MotorRight_On();
            USART.Print_String("Motor RIGHT (Azimuth right) is On now.\r");
        }
        break;
        case 12: { // MotorRtOf
            MotorRight_Off();
            USART.Print_String("Motor RIGHT (Azimuth right) is Off now.\r");
        }
        break;
        default: return; break;
    }
}

void cmd_t::LabServer_Process_Byte(uint16_t Byte) {
//    if (Byte == 33) {
//        LCD.PrintString(3,4, "Ep!", false);
//    } else {
//    if ((Byte == '1') || (Byte == '2') || (Byte == '3')) {
//        if (Byte == '1') {
//            LCD.PrintString(3,5, "GetParam", false);
//            return;
//        } else if (Byte == '2'){
//            Set_Parametres();
//            return;
//        } else if (Byte == '3') {
//            LCD.PrintString(3,5, "  Reset ", false);
//        }
//    } else {
//        LCD.PrintString(3,7, "Another", false);
//        LCD.PrintInt(0,1,Byte,false);
//        return;
//    }
//    }
    switch (Byte) {
        case 'T': {
            USART.Print_String("\rSwitch to Terminal - OK\r");
            parametres.cmd_sourse = Terminal;
            parametres.cmd_switch_pointer = true;
            Active_window.Search(window.pointer);
        }
            break;
        case 'w': {
            LCD.PrintString(3,5, "Move", false);
            MotorUp_On();
            return;
        }
            break;
        case 'a': {
            LCD.PrintString(3,5, "Move", false);
            MotorLeft_On();
            return;
        }
            break;
        case 's': {
            LCD.PrintString(3,5, "Move", false);
            MotorDown_On();
            return;
        }
            break;
        case 'd': {
            LCD.PrintString(3,5, "Move", false);
            MotorRight_On();
            return;
        }
            break;
        case cmd_get: {
            Get_Parametres();
            LCD.PrintString(3,5, "GetParam", false);
            cmd_flag = get_parametres;
        }
            break;
        case cmd_set: {
            Set_Parametres();
            cmd_flag = set_parametres;
        }
            break;
        case cmd_reset: {
            LCD.PrintString(3,5, "  Reset ", false);
            USART.Print_String("Reset\r");
            cmd_flag = reset;
            return;
        }
            break;
        default: {
            cmd_flag = reset;
            return;
        }
    }
} // LabServer Process byte

void cmd_t::Clear_buffer(void) {
    uint8_t i;
        for (i=0; i<lengh_of_cmd; i++) {
            CMD_Var.Arr[i] = 0;
        } // for i
}

void cmd_t::Get_Parametres(void){ // Parametres will be send to LabServer
    extern uint16_t adc01_angle, adc02_angle;
    Event.ADC_get();
    USART.Send(adc01_angle); // Send angle of azimuth
    USART.Send(adc02_angle); // Send angle of elevation
} // Get parametres

void cmd_t::Set_Parametres(void) { // Set Parametres from LabServer
    extern uint16_t new_azimuth, new_elevation;
    new_azimuth = 0;
    new_elevation = 0;
    if (parametres.cmd_sourse == LabServer) {
        do {
            if (!USART.Reset()) {
                while (USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET);
                new_azimuth = USART_ReceiveData(USART1);;
            }
            else return;
        } while (new_azimuth == 0);
        do {
            if (!USART.Reset()) {
                while (USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET);
                new_elevation = USART_ReceiveData(USART1);;
            }
            else return;
        } while (new_elevation == 0);
        LCD.PrintString(3,5, "SetParam", false);
        LCD.Clear_zone(0,6,11);
        LCD.PrintUint(2,6, new_azimuth, false);
        LCD.PrintUint(8,6, new_elevation, false);
    } // LabServer
    else {
        if (parametres.cmd_sourse == Terminal) {
            do {
                    USART.Terminal_Angle_Task();
                    if (cmd_flag == reset) {
                        cmd_flag = free;
                        return;
                    }
            } while ((new_azimuth == 0) || (new_elevation == 0));
        } // Terminal
    }

    USART.Print_String("Angles recieved.\r");
    USART.Print_Uint(new_azimuth);
    USART.Print(' ');
    USART.Print_Uint(new_elevation);
    USART.Print('\r');
    Set_Angles(new_azimuth, new_elevation);
} // Set parametres


void cmd_t::Set_Angles(uint16_t new_azimuth, uint16_t new_elevation) {
    uint32_t setup_tmr;
    Event.ADC_get();
    az_data = ADC.to_AAZ_Lab(adc01_data);
    el_data = ADC.to_AEL_Lab(adc02_data);
    angle_flag.azimuth_set = false;
    angle_flag.elevation_set = false;

    do {
        if (!USART.Reset()) {
            Event.ADC_get();
            az_data = ADC.to_AAZ_Lab(adc01_data);
                if (new_azimuth == az_data) angle_flag.azimuth_set = true;
                else if ( ( ((az_data >= 0) && (az_data < 60)) && ((new_azimuth >= 0) && (new_azimuth <= 60)) ) ||
                              ( ((az_data > 196) && (az_data < 256)) && ((new_azimuth > 196) && (new_azimuth < 256)) ) )
                     {
                        if (az_data > new_azimuth) MotorLeft_On();
                        else if (az_data < new_azimuth) MotorRight_On();
                     }

                     else if ( ((az_data >= 196) && (az_data < 256)) && ((new_azimuth >= 0) && (new_azimuth <= 60)) ) MotorRight_On();

                          else if ( ((az_data >= 0) && (az_data <= 60)) && ((new_azimuth >= 196) && (new_azimuth < 256)) ) MotorLeft_On();
                if (Delay.Elapsed(&setup_tmr, 10)) {
                    MotorRight_Off();
                    MotorLeft_Off();
                    Delay.us(9);
                }
                LCD.Clear_zone(2,7,4);
                LCD.PrintUint(2,7, az_data, false);
        } else return;
    } while (!angle_flag.azimuth_set);
    MotorLeft_Off();
    MotorRight_Off();
    do {
        if (!USART.Reset()) {
            Event.ADC_get();
            el_data = ADC.to_AEL_Lab(adc02_data);
                if (new_elevation == el_data) angle_flag.elevation_set = true;
                    else if ( ( ((el_data >= 1) && (el_data < 30)) && ((new_elevation >= 1) && (new_elevation < 30)) ) ||
                            ( ((el_data > 230) && (el_data <= 256)) && ((new_elevation > 230) && (new_elevation <= 256)) ) )
                    {
                        if (el_data > new_elevation) MotorUp_On();
                        else if (el_data < new_elevation) MotorDown_On();
                    }

                        else if ( ((el_data >= 1) && (el_data <= 30)) && ((new_elevation > 230) && (new_elevation <= 256)) ) MotorUp_On();

                             else if ( ((el_data > 230) && (el_data <= 256)) && ((new_elevation >= 1) && (new_elevation < 30)) ) MotorDown_On();
                if (Delay.Elapsed(&setup_tmr, 10)) {
                    MotorUp_Off();
                    MotorDown_Off();
                    Delay.us(9);
                }
                LCD.Clear_zone(0,7,4);
                LCD.PrintUint(8,7, el_data, false);
        } else return;
    } while (!angle_flag.elevation_set);
    MotorUp_Off();
    MotorDown_Off();

    angle_flag.azimuth_set = false;
    angle_flag.elevation_set = false;
    new_azimuth = 0;
    new_elevation = 0;
    USART.Print_String("Setup completed.\r");
}


/*
            if (!angle_flag.azimuth_set) {
                if (ADC_var.Azumuth_below_0) adc01_angle = 256 - adc01_angle;
                    if (new_azimuth == adc01_angle)  {
                        angle_flag.azimuth_set = true;
                    } else {
                        if ((new_azimuth >=0) && (new_azimuth<=60)) {
                        if (new_azimuth > adc01_angle) {
                                MotorLeft_On();
                        } else if (new_azimuth < adc01_angle) {
                                MotorRight_On();
                        }
                    } else if ((new_azimuth >=195) && (new_azimuth<=256)) {
                        if (new_azimuth > adc01_angle) {
                                MotorRight_On();
                        } else if (new_azimuth < adc01_angle) {
                                MotorLeft_On();
                        }
                    } else {
                        USART.Print_String("Wrong azimuth!\r");
                        return;
                    } // if wrong azimuth
                } // if not reached
            } else {
                if (!angle_flag.elevation_set) {
                    if (ADC_var.Elevation_below_0) adc02_angle = 256-adc02_angle;
                        if (new_elevation == adc02_angle) {
                            angle_flag.elevation_set = true;
                        } else {
                            if ((!angle_flag.el_dec) && (new_elevation >= 0) && (new_elevation <= 30)) {
                            if (new_elevation > adc02_angle) {
                                MotorDown_On();
                            } else if (new_elevation < adc02_angle) {
                                MotorUp_On();
                            }
                        } else if ((angle_flag.el_dec) && (new_elevation >=245) && (new_elevation <=256)) {
                            if (new_elevation > adc02_angle) {
                                MotorUp_On();
                            } else if (new_elevation < adc02_angle) {
                                MotorDown_On();
                            }
                        } else {
                            USART.Print_String("Wrong elevation!\r");
                            USART.Print(new_elevation);
                            USART.Print('\r');
                            return;
                        } // if wrong elevation
                    } // in not reached
                } // if not set
            } // if azimuth set
        } // if not Reset
        else return;
    } while ((!angle_flag.azimuth_set) || (!angle_flag.elevation_set));
} // Setup Angles
*/
